#include "WPILib.h"
#include "math.h"
#include "Shooter.hpp"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
#define GATE

#define WAITTIME 0.005
#define GATESTARTTIME -0.25
#define GATEENDTIME 0.75
#define GATEDRIVE .6
class RobotDemo : public SimpleRobot
{
	RobotDrive myRobot; // robot drive system
	Joystick stick; // only joystick
	Shooter* myShooter;
#ifdef GATE
	CANJaguar GateJag;
#endif

public:
	RobotDemo(void):
		myRobot(1,2)	// these must be initialized in the same order
		,stick(1)		// as they are declared above.
#ifdef GATE
		,GateJag(9)
#endif
	{
		myRobot.SetSafetyEnabled(false);
		myRobot.SetExpiration(0.1);
		
		printf("\n\nShooter driven by Camera\n\n");
		
		myShooter = new Shooter(3, 7);
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		myRobot.SetSafetyEnabled(false);
		myRobot.Drive(0.5, 0.0); 	// drive forwards half speed
		Wait(2.0); 				//    for 2 seconds
		myRobot.Drive(0.0, 0.0); 	// stop robot
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		printf("OperatorControl started\n");
		
		myRobot.SetSafetyEnabled(false);
#ifdef GATE
		float GateTime=GATEENDTIME,GateDrive=0.0;
#endif
		while (IsOperatorControl())
		{
			
			// Gate Drive**********************************************
#ifdef GATE
			if((stick.GetRawAxis(3)!=0.0)&&(GateTime>=(GATEENDTIME-WAITTIME)))GateTime=GATESTARTTIME;
			GateTime=GateTime+WAITTIME;
			if (GateTime>GATESTARTTIME)
			{
				GateDrive=GATEDRIVE;
			}
			if(GateTime>0)
			{
				GateDrive=-GATEDRIVE;
			}
			if(GateTime>GATEENDTIME)
			{
				GateDrive=0.0;
				GateTime=GateTime-WAITTIME;
			}
			GateJag.Set(GateDrive);
#endif
			// End Gate Drive *****************************************
			
			float cameraY = 0.2*stick.GetRawAxis(2); //    2: Left Stick Y Axis
					
			if(fabs(cameraY) > 0.02)	// temp hard coded deadband
			{
				//printf("Calling myShooter.ControlByCamera - cameraY = %0.2f\n", cameraY);
				myShooter->ControlByCamera(.5*cameraY);
			}
			
			//*** TEMP For calibration ***
			float shooterSpeedY = 0.1*stick.GetRawAxis(5);//    5: Right Stick Y Axis
			if(fabs(shooterSpeedY) > 0.1)	// temp hard coded deadband
			{
				//printf("Calling myShooter.ControlByCamera - cameraY = %0.2f\n", cameraY);
				myShooter->ManualControlIncremental(shooterSpeedY);
			}
			
			if(stick.GetRawButton(1) == 1) // Temp read A button
			{
				printf("\nCalibrate Camera angle to 0 deg\n");
				myShooter->CalibrateCameraAngle(0);
			}
				myShooter->PrintFF();

			Wait(WAITTIME);				// wait for a motor update time
		}
	}
};

START_ROBOT_CLASS(RobotDemo);

